#include <string.h>

#include "pbd_pblock.h"
#include "pbd_error.h"

int pbd_pblock_append(pbd_pblock_t *pblock, uint8 *buf, int buflen)
{
	if (pblock->clen + buflen > pblock->len) {
		pbd_print(PBD_LOG_ERR, PBD_FUN, "Pblock Buffer Not Enough\n");
		return -1;
	}

	memcpy(pblock->data + pblock->clen, buf, buflen);
	pblock->clen += buflen;

	return 1;
}

int pbd_pblock_add(pbd_pblock_t *pblock_head, pbd_pblock_t *pblock)
{
	pbd_pblock_t *cur;

	if (!pblock_head || !pblock || pblock_head == pblock) {
		pbd_print(PBD_LOG_WARNING, PBD_FUN, "Add Empty Pblock\n");
		return -1;
	}

	for (cur = pblock_head; cur->next || cur != pblock_head; cur = cur->next) {
		if (cur->layer > pblock->layer){
			if (cur->prev->layer == pblock->layer)
				pblock->id = cur->prev->id + 1;
			else
				pblock->id = 1;

			// link
			cur->prev->next = pblock;
			pblock->prev = cur->prev;
			pblock->next = cur;
			cur->prev = pblock;
			return 1;
		}
	}

	// link at the end
	pblock->id = 1;
	cur->next = pblock;
	pblock->prev = cur;
	return 1;
}

int pbd_pblock_rem(pbd_pblock_t *pblock_head, pbd_pblock_t *pblock)
{
	if (!pblock_head || !pblock || pblock_head == pblock) {
		pbd_print(PBD_LOG_WARNING, PBD_FUN, "Remove Empty Pblock\n");
		return -1;
	}

	// if pblock at the start
	if (pblock->prev)
		pblock->prev->next = pblock->next;
	else
		pblock_head->next = pblock->next;
	// if pblock at the last
	if (pblock->next)
		pblock->next->prev = pblock->prev;
	

	if (pblock->data)
		free_pbd_mem((void *)pblock->data);
	free_pbd_mem((void *)pblock);

	return 1;
}

int pbd_pblock_rem_itr(pbd_pblock_t *pblock_head, pbd_pblock_t *pblock)
{
	pbd_pblock_t *pblock_prev;

	if (!pblock_head || !pblock || pblock_head == pblock) {
		pbd_print(PBD_LOG_WARNING, PBD_FUN, "Remove Empty Pblock\n");
		return -1;
	}

	// if pblock at the start
	if (pblock->prev) {
		pblock_prev = pblock->prev;
		pblock->prev->next = pblock->next;
	} else {
		pblock_prev = pblock_head;
		pblock_head->next = pblock->next;
	}
	
	// if pblock at the last
	if (pblock->next)
		pblock->next->prev = pblock->prev;
	

	if (pblock->data)
		free_pbd_mem((void *)pblock->data);
	free_pbd_mem((void *)pblock);

	return 1;
}


pbd_pblock_t *pbd_pblock_new(int len, int layer, int type, uint8 *buf, int buflen)
{
	pbd_pblock_t *pblock;

	// sanity check
	if (len < buflen)
		return NULL;
	if (!(pblock = (pbd_pblock_t *)alloc_pbd_mem(sizeof(pbd_pblock_t))) || !(pblock->data = alloc_pbd_mem(len)))
		return NULL;

	pblock->layer = layer;
	pblock->type = type;
	pblock->len = len;
	memcpy(pblock->data, buf, buflen);
	pblock->clen = buflen;

	return pblock;
}

void pbd_pblock_free(pbd_pblock_t *pblock)
{
	if (pblock) {
		if (pblock->data)
			free_pbd_mem((void *)pblock->data);
		free_pbd_mem((void *)pblock);
	}
}

uint8 *pbd_pblock_gather(pbd_pblock_t *pblock_head, int *size)
{
	pbd_pblock_t *pblock;
	// non-re-entrant!
	static uint8 buf[1500] = { 0 };
	int buf_mark = 0;

	PBLOCK_ITER(pblock_head, pblock) {
		if (pblock->data && pblock->clen) {
			if (buf_mark + pblock->clen > 1500) {
				pbd_print(PBD_LOG_WARNING, PBD_FUN, "Not Enough Buffer For Packet\n");
				*size = buf_mark;
				return buf;
			}
			
			memcpy(buf + buf_mark, pblock->data, pblock->clen);
			buf_mark += pblock->clen;
		}
	}

	*size = buf_mark;
	return buf;
}

